/*
 * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
 *
 * This file is part of SeMoLearning.
 *
 * SeMoLearning is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SeMoLearning is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SeMoLearning.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "semol/core/SemolExp.h" // DLL export/import defines

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "semol/core/Sensor.h"
#include "semol/core/Actuator.h"
#include <map>
#include <list>
#include <vector>
#include <set>

//TODO : add copy constructor

namespace semol
{

class SEMOL_EXP NeighborLink
{
public:
	NeighborLink(int toId,
			const std::list<std::vector<int> > & sensorHistoryIds = std::list<std::vector<int> >(),
			const std::list<Actuator> & actuators = std::list<Actuator>(),
			int actuatorId = 0) :
		_toId(toId),
		_actuatorId(actuatorId),
		_actuators(actuators),
		_sensorHistoryIds(sensorHistoryIds)
	{}
	virtual ~NeighborLink() {}

	int toId() const {return _toId;}
	int actuatorId() const {return _actuatorId;}
	const std::list<Actuator> & actuators() const {return _actuators;}
	const std::list<std::vector<int> > & sensorHistoryIds() const {return _sensorHistoryIds;}

	bool isSensorHistorySameIds() const;

protected:
	friend class Node;
	int setId(int id) {return _toId = id;}
	bool updateSensorHistoryIds(int idFrom, int idTo);

private:
	int _toId;
	int _actuatorId;
	std::list<Actuator> _actuators;
	std::list<std::vector<int> > _sensorHistoryIds; // last is the nearest
};

typedef std::multimap<int, NeighborLink> NeighborsMultiMap;

class SEMOL_EXP Node
{
public:
	Node(int id, const std::vector<int> & sensors = std::vector<int>());
	virtual ~Node();

	int id() const {return _id;}

	void addNeighbors(const NeighborsMultiMap & neighbors);
	void addNeighbor(const NeighborLink & neighbor);
	void removeNeighbor(int neighborId, int actuatorId = 0); // actuatorId = -1 means all neighbors with neighborId
	void removeNeighbors();
	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
	void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
	void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
	void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_neighborsModified=true;}
	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
	bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
	void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
	void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_neighborsModified=true;}
	void setSaved(bool saved) {_saved = saved;}
	void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
	void changeNeighborIds(int idFrom, int idTo);
	void keepUniqueLink(int id);

	const NeighborsMultiMap & getNeighbors() const {return _neighbors;}
	const std::set<int> & getNeighborsWithActuators() const {return _neighborsWithActuators;}
	const std::set<int> & getNeighborsAll() const {return _neighborsAll;}
	const std::set<int> & getAids() const {return _aids;}
	int getWeight() const {return _weight;}
	const std::set<int> & getLoopClosureIds() const {return _loopClosureIds;}
	const std::set<int> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
	bool isSaved() const {return _saved;}
	bool isModified() const {return _modified || _neighborsModified;}
	bool isNeighborsModified() const {return _neighborsModified;}

	bool isEmpty() const;

	void setSensors(const std::vector<int> & sensors) {_sensors = sensors;}
	const std::vector<int> & getSensors() const {return _sensors;}
	const std::map<int, int> & getSensorsChanged() const {return _sensorsChanged;}
	void changeSensorsRef(int oldSensorId, int activeSensorId);

private:
	int _id;
	NeighborsMultiMap _neighbors; // id, neighborLink
	std::set<int> _neighborsWithActuators; // Hack, to increase efficiency of Memory::getNeighborIds()
	std::set<int> _neighborsAll; // Hack, to increase efficiency of Memory::getNeighborIds()
	std::set<int> _aids; // Action ids, Hack, to increase efficiency of Memory::getNeighborIds()
	int _weight;
	std::set<int> _loopClosureIds;
	std::set<int> _childLoopClosureIds;
	bool _saved; // If it's saved to db
	bool _modified;
	bool _neighborsModified; // Optimization when updating signatures in database

	std::vector<int> _sensors;
	std::map<int, int> _sensorsChanged; // <oldId, newId>
};


} // namespace rtabmap
